#include "motor_RS485.hpp"
#include <cstdint>
#include <iostream>
#include <modbus/modbus-rtu.h>
#include <modbus/modbus.h>
#include <unistd.h>

static constexpr int MODBUS_ERROR = -1;
using namespace std;

Motor_RS485::Motor_RS485()
{
    modBusCtx = nullptr;
    hasError = true;
}

Motor_RS485::~Motor_RS485()
{
    if (modBusCtx == nullptr)
    {
        return;
    }
    if (!useSharedCtx)
    {
        modbus_close(modBusCtx);
        modbus_free(modBusCtx);
    }
}

bool Motor_RS485::InitBySerialDevice(std::string dev, int baud, MotorConfig cfg, bool debug)
{
    modBusCtx = CreatModBusInstance(dev, baud, debug);
    if (modBusCtx == nullptr)
        return false;

    bool state = InitDevice(modBusCtx, cfg);
    useSharedCtx = false;
    Select();
    return state;
}

bool Motor_RS485::InitDevice(modbus_t *modbus, MotorConfig cfg)
{
    useSharedCtx = true;
    if (modbus == nullptr)
    {
        return false;
    }
    modBusCtx = modbus;
    config = cfg;
    hasError = false;
    return true;
}

bool Motor_RS485::CheckDevice()
{
    return hasError;
}

float Motor_RS485::GetRPM()
{
    uint16_t buffer[2];
    int ret = modbus_read_input_registers(modBusCtx, static_cast<int>(RegeisterAddress::RealERPM), 2, buffer);
    if (ret == MODBUS_ERROR)
        throw "GET RPM ERROR";
    int32_t erpm = buffer[0] << 16 | buffer[1];
    // cout << std::hex << erpm << ":" << std::oct << (float)erpm / config.poleCount << endl;
    return (float)erpm / config.poleCount;
}

bool Motor_RS485::SetControlMode(ControlMode mode)
{
    if (hasError)
    {
        return false;
    }
    return MODBUS_ERROR == modbus_write_register(modBusCtx, static_cast<int>(RegeisterAddress::ControlMode),
                                                 static_cast<uint16_t>(mode));
}

void Motor_RS485::SetRPM(float value)
{
    int32_t erpm = value * config.poleCount;
    uint16_t buffer[2];
    buffer[0] = erpm >> 16 & 0xFFFF;
    buffer[1] = erpm & 0xFFFF;
    int ret = modbus_write_registers(modBusCtx, static_cast<int>(RegeisterAddress::SetERPM), 2, buffer);
    if (ret == MODBUS_ERROR)
        throw "SET RPM ERROR";
}
void Motor_RS485::SetPWM(float precent)
{
    precent = precent > 1000 ? 1000 : precent;
    precent = precent < -1000 ? -1000 : precent;
    int16_t pwm = precent;
    int ret = modbus_write_register(modBusCtx, static_cast<int>(RegeisterAddress::SetDuty), pwm);
    if (ret == MODBUS_ERROR)
        throw "SET PWM ERROR";
}

void Motor_RS485::SetCurrent(float mA)
{
    // todo check overlap
    int16_t pwm = (mA / 10);
    int ret = modbus_write_register(modBusCtx, static_cast<int>(RegeisterAddress::SetCurrent), pwm);
    if (ret == MODBUS_ERROR)
        throw "SET PWM ERROR";
}
void Motor_RS485::SendHeartBeat()
{
    heartBeatValue++;
    int ret = modbus_write_register(modBusCtx, static_cast<int>(RegeisterAddress::HeartBeat), heartBeatValue);
    if (ret == MODBUS_ERROR)
        throw "SET HearBeat ERROR";
}

void Motor_RS485::Select()
{
    usleep(2000);
    modbus_set_slave(modBusCtx, config.address);
}

modbus_t *Motor_RS485::CreatModBusInstance(std::string dev, int baudrate, bool debug)
{
    modbus_t *modBusCtx;
    modBusCtx = modbus_new_rtu(dev.c_str(), baudrate, 'N', 8, 1);
    if (modBusCtx == nullptr)
    {
        cout << "Creat modbus fail" << endl;
        return nullptr;
    }
    modbus_set_debug(modBusCtx, debug);
    modbus_set_response_timeout(modBusCtx, 0, 10000);
    modbus_rtu_set_serial_mode(modBusCtx, MODBUS_RTU_RS485);

    int ret = modbus_connect(modBusCtx);
    if (ret != 0)
    {
        cerr << "mod-bus connect error" << endl;
        modbus_free(modBusCtx);
        return nullptr;
    }
    return modBusCtx;
}
